﻿using DotNetMatrix;
using Server.ChartLog;
using Server.Core.Filter;
using Server.MessageModule.Message;
using Server.Utis;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Server.MessageModule.MessageProcessor
{
    class SensorProcessor : BaseMessageProcessor
    {
        public ChartDisplay _chart = null;
        private Boolean _isCaptureMode = false;
        private float[] _captureOrientation = null;

        private Boolean _isFilter = false;
        private TheBestKalman _kalmanFilterX;
        private TheBestKalman _kalmanFilterY;
        private TheBestKalman _kalmanFilterZ;

        public SensorProcessor()
            : base(ShareMacro.MESSAGETYPE_SENSOR)
        {
            // init kalman filter
            _kalmanFilterX = new TheBestKalman(1, 1, 1, 1, 0.05, 0.4);//2, 2, 0.1, 1, 20
            _kalmanFilterY = new TheBestKalman(1, 1, 1, 1, 0.05, 0.4);//2, 2, 0.1, 1, 20
            _kalmanFilterZ = new TheBestKalman(1, 1, 1, 1, 0.05, 0.4);//2, 2, 0.1, 1, 20
            //_kalmanFilter = new KalmanFilter2(0.0625, 32.0, 1.3833094);
        }

        public override void onReceiptMessage(INetworkMessage msg)
        {
            string msgType = msg.getType();
            if (msgType != ShareMacro.MESSAGETYPE_SENSOR)
                return;

            SensorMessage sensor = msg as SensorMessage;
            if (sensor == null)
                throw new Exception("Invalid type message");

            preSensorData(ref sensor);

            if (_chart != null)
            {
                _chart.pushSensorData(sensor);
            }
        }

        public void preSensorData(ref SensorMessage sensor)
        {
            // Capture process
            if (_isCaptureMode)
            {
                if (_captureOrientation == null)
                {
                    Dictionary<string, float> orgiData = sensor.MagOrientation;
                    _captureOrientation = new float[3];
                    _captureOrientation[0] = orgiData["x"];
                    _captureOrientation[1] = orgiData["y"];
                    _captureOrientation[2] = orgiData["z"];

                    System.Console.WriteLine("Set capture orientation successfully!");
                }

                // Current orienttation
                Dictionary<string, float> orgi = sensor.MagOrientation;
                double radX, radY, radZ;
                radX = orgi["x"];
                radY = orgi["y"];
                radZ = orgi["z"];

                GeneralMatrix rotaM1 = MatrixUtils.getReverseRotateMatrix(radY, radZ, radX);//radY, radZ, radX
                GeneralMatrix rotaM2 = MatrixUtils.getRotateMatrix(_captureOrientation[1], _captureOrientation[2], _captureOrientation[0]);//radY, radZ, radX

                Dictionary<string, float> accData = sensor.Accel;
                
                GeneralMatrix accelMatrix = new GeneralMatrix(1, 3);
                accelMatrix.SetElement(0, 0, accData["x"]);
                accelMatrix.SetElement(0, 1, accData["y"]);
                accelMatrix.SetElement(0, 2, accData["z"]);

                accelMatrix = accelMatrix.Multiply(rotaM1);
                accelMatrix = accelMatrix.Multiply(rotaM2);
                Dictionary<string, float> newAccData = new Dictionary<string, float>();
                newAccData["x"] = (float)accelMatrix.GetElement(0, 0);
                newAccData["y"] = (float)accelMatrix.GetElement(0, 1);
                newAccData["z"] = (float)accelMatrix.GetElement(0, 2);
                sensor.Accel = newAccData;
            }
            else
            {
                _isCaptureMode = false;
                _captureOrientation = null;
            }

            //sensor.Accel["y"] = sensor.Accel["x"];
            //sensor.MagOrientation["y"] = sensor.MagOrientation["x"];
            // filter process
            if (_isFilter)
            {
                sensor.Accel["xk"] = (float)_kalmanFilterX.processData(sensor.Accel["x"]);
                sensor.Accel["yk"] = (float)_kalmanFilterY.processData(sensor.Accel["y"]);
                sensor.Accel["zk"] = (float)_kalmanFilterZ.processData(sensor.Accel["z"]);
                //sensor.MagOrientation["y"] = (float)_kalmanFilter.processData(sensor.MagOrientation["y"]);
                //sensor.MagOrientation["z"] = (float)_kalmanFilter.processData(sensor.MagOrientation["z"]);
            }
            else
            {
                sensor.Accel["xk"] = 0;
                sensor.Accel["yk"] = 0;
                sensor.Accel["zk"] = 0;
            }
        }

        public void setCaptureMode(Boolean value)
        {
            _isCaptureMode = value;
        }

        public void setFilterMode(Boolean value)
        {
            _isFilter = value;
            System.Console.WriteLine("Set filter mode successfully!");
        }
    }
}
